package org.nashua.tt151;

import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.SimpleRobot;


public class Drive extends SimpleRobot {  
    Joystick stick = new Joystick(1);
    Jaguar north = new Jaguar(4), east = new Jaguar(2), west = new Jaguar(3);
    Jaguar south = new Jaguar(1);
    
    public void operatorControl() {
        
        double vPowerDest = 0;
        double vPower = 0, hPower = 0;
        
        while(isEnabled() && isOperatorControl()){
            
            hPower = stick.getRawAxis(3);
            vPowerDest = stick.getRawAxis(4);
            vPower += (vPowerDest-vPower)/100;
            System.out.println("hpower:"+hPower);
            System.out.println("vpower:"+vPower);
            
            if(stick.getRawButton(6)){
                north.set(.75);
                east.set(.75);
                south.set(.75);
                west.set(.75);
            }else if(stick.getRawButton(5)){
                north.set(-.75);
                east.set(-.75);
                south.set(-.75);
                west.set(-.75);
            }else{
                north.set(hPower);
                east.set(vPower);
                south.set(-hPower);
                west.set(-vPower);
            }
        }
    } 
}
